Đồ án 2 robot hai bánh tự cân bằng sử dụng giải thuật lqr

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG ĐỒ ÁN ROBOT HAI BÁNH TỰ CÂN BẰNG SỬ DỤNG GIẢI THUẬT LQR Ngành Kỹ Thuật Điều Khiển & Tự Động Hóa GVHD: TS NGUYỄN TRẦN MINH NGUYỆT Sinh viên: NGUYỄN HƯNG MSSV: 17151086 HỒ LÊ LONG THIÊN MSSV: 17151132 TP HỒ CHÍ MINH, THÁNG NĂM 2020 TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG ĐỒ ÁN ROBOT HAI BÁNH TỰ CÂN BẰNG SỬ DỤNG GIẢI THUẬT LQR Ngành Kỹ Thuật Điều Khiển & Tự Động Hóa GVHD: TS NGUYỄN TRẦN MINH NGUYỆT Sinh viên: NGUYỄN HƯNG MSSV: 17151086 HỒ LÊ LONG THIÊN MSSV: 17151132 TP HỒ CHÍ MINH, THÁNG NĂM 2020 MỤC LỤC CHƯƠNG GIỚI THIỆU ERROR! BOOKMARK NOT DEFINED 1.1 TẦM QUAN TRỌNG CỦA ĐỀ TÀI ERROR! BOOKMARK NOT DEFINED 1.2 MỤC TIỆU 1.3 GIỚI HẠN 1.4 PHƯƠNG PHÁP THỰC HIỆN 1.5 NỘI DUNG CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1 MƠ HÌNH HĨA XE HAI BÁNH TỰ CÂN BẰNG 2.2 LÝ THUYẾT LQR 2.2.1 Giới thiệu điều khiển tồn phương tuyến tính LQR 2.2.2 Phương trình Riccati đại số 2.2.3 Bài tốn LQR liên tục 2.2.4 Bài toán LQR rời rạc 2.3 MPU – 6050 2.3.1 Giới thiệu MPU – 6050 2.3.2 Xử lý liệu đọc từ cảm biến 2.3.3 Các phương pháp xử lý tín hiệu từ cảm biến MPU – 6050 10 2.3.3.1 Lọc bổ phụ thông tần 10 2.3.3.2 Lọc Kalman 11 2.4 CẢM BIẾN ĐO VỊ TRỊ ENCODER 15 2.4.1 Giới thiệu 15 2.4.2 Xử lý tín hiệu Encoder 15 ĐỘNG CƠ ĐIỆN MỘT CHIỀU 16 2.5 2.5.1 Giới thiệu 16 2.5.2 Điều chỉnh tốc độ động chiều 18 2.6 GIAO TIẾP I2C 19 2.6.1 Nguồn gốc I2C 19 2.6.2 Cấu tạo nguyên lý làm việc 20 2.6.3 Giao thức truyền liệu 22 CHƯƠNG THIẾT KẾ HỆ THỐNG 22 3.1 NGUYÊN LÝ HOẠT ĐỘNG ROBOT BÁNH TỰ CÂN BẰNG 22 3.2 LƯU ĐỒ GIẢI THUẬT 23 3.3 MƠ HÌNH ROBOT BÀNH TỰ CÂN BẰNG 24 3.4 LỰA CHỌN LINH KIỆN 25 3.4.1 Pin động 25 3.4.2 Mạch điều khiển 27 3.4.3 Mạch giảm áp 28 3.4.4 Mạch cầu H 29 3.4.5 Cảm biến góc 31 3.5 SƠ ĐỒ KẾT NỐI ERROR! BOOKMARK NOT DEFINED 3.6 MƠ HÌNH THỰC ERROR! BOOKMARK NOT DEFINED.33 CHƯƠNG KẾT QUẢ 34 TÀI LIỆU THAM KHẢO ERROR! BOOKMARK NOT DEFINED.35 PHỤ LỤC 36 CHƯƠNG GIỚI THIỆU 1.1 TẦM QUAN TRỌNG CỦA ĐỀ TÀI Những xe robot ngành xây dựng, nhiệm vụ tuần tra, giám sát, điều khiển từ xa hầu hết robot di chuyển ba bánh xe, với hai bánh lái lắp ráp đồng trục bánh đuôi nhỏ Có nhiều kiểu khác nhau, kiểu thơng dụng Cịn xe bánh, thường đầu xe có hai bánh truyền động đầu xe lại gắn hai bánh lái Hình 1.1: Robot ghi hình điều khiển từ xa WowWee Việc thiết kế ba hay bốn bánh làm cho xe robot thăng ổn định nhờ trọng lượng chia cho hai bánh lái bánh hay khác để đỡ trọng lượng xe Nếu trọng lượng đặt nhiều vào bánh lái xe khơng ổn định dễ bị ngã, cịn đặt nhiều vào bánh hai bánh khả bám Nhiều thiết kế xe robot di chuyển tốt địa hình phẳng, khơng thể di chuyển tốt địa hình lồi lõm mặt phẳng nghiêng Khi di chuyển lên đồi, trọng lượng xe robot dồn vào đuôi xe làm bánh lái khả bám trượt ngã, bậc thang, chí dừng hoạt động quay trịng bánh xe Một ví dụ nhược điểm xe ba bốn bánh là: Khi xe lên dốc nghiêng, trọng lượng đầu xe nhẹ so với tổng khối lượng xe, cộng thêm việc động bánh sau mạnh, khiến cho xe thăng lật sau Hình 1.2: Xe bánh di chuyển mặt phẳng nghiêng, bị lật sau Việc chế tạo xe bánh, giống xe đồ chơi hay loại xe bốn bánh sử dụng giao thông gặp vấn đề trên, điều làm xe robot không gọn gàng phận lái (cua quẹo) gặp chút khó khăn, khó xác định góc cua quãng đường Các xe dạng hai bánh đồng trục lại thăng tốt linh động di chuyển địa hình phức tạp Khi leo sườn dốc, tự động nghiêng trước giữ cho trọng lượng dồn về trước Tương tự leo xuống dốc, nghiêng sau dồn trọng lực sau Do đó, khơng có tượng xe thăng ngã Hình 1.3: Xe bánh di chuyển mặt phẳng nghiêng Đối với địa hình khó đi, thăng xe hai bánh mang lại nhiều ý nghĩa thực tiễn giới hạn ổn định xe ba/bốn bánh truyền thống Ngoài ra, đề tài “xe hai bánh tự cân bằng” có nhiều ứng dụng đời sống làm tảng cho ứng dụng khác dựa vào thiết kế 1.2 MỤC TIÊU - Đề tài tập trung vào việc xây dựng mơ hình thuật tốn điều khiển hai bánh tự cân phương pháp LQR - Thiết kế, chế tạo mô hình thực tế có khả giữ thăng nhiều địa hình khác 1.3 GIỚI HẠN ĐỀ TÀI Trong khoản thời gian 16 tuần nghiên cứu thực đồ án, nhóm sinh viên gặp phải giới hạn đề tài sau: - Vì khơng có nhiều kinh phí nên mơ hình đề tài khơng lớn hồnh tráng, mặt trang trí khơng đầu tư nhiều - Khả lập trình, tính tốn cịn hạn chế nên thuật tốn code điều hành xe robot chưa chỉnh chu, chi tiết xác hồn tồn 1.4 PHƯƠNG PHÁP THỰC HIỆN Bản thân hệ phi tuyến ảnh hưởng yếu tố vị trí, vận tốc, góc nghiêng đồng thời nên trình điều khiển ổn định hệ thống gặp nhiều khó khăn Q trình nghiên cứu để thực đề tài tóm tắt qua bước sau: - Phân tích ưu nhược điểm đề tài, ứng dụng thực tiễn liên quan đến đề tài - Đánh giá tính khả phát triển tương lai - Thu thập tài liệu ngồi nước mơ hình phương pháp điều khiển - Thiết kế mơ hình phần cứng - Thiết kế phần điện sử dụng module có sẵn - Thực thi giải thuật lập trình - Kiểm tra sửa lỗi 1.5 NỘI DUNG Chương 1: Tổng quan Giới thiệu tổng quan robot hai bánh tự cân bằng, ưu khuyết điểm thiết kế hai bánh Chương 2: Cơ sở lí thuyết Phương pháp điều khiển PID, Fuzzy, LQR, Giới thiệu Kalman, Xử lí tín hiệu MPU 6050 encoder, Ardunio mega 2560 Chương 3: Thiết kế hệ thống Chi tiết thiết kế khí, thiết kế điện, lưu đồ giải thuật, mơ giải thuật Simulink Chương 4: Kết Kết thực nghiệm hướng phát triển đề tài Hình 3.8: Arduino Mega 2560 3.4.3 MẠCH GIẢM ÁP Bộ nguồn vi điều khiển mạch điện tử tách rời khỏi nguồn cung cấp cho động nhằm hạn chế tín hiệu nhiễu từ động lên hoạt động vi điều khiển cảm biến Mạch giảm áp LM2596 3A: - Là module giảm áp có khả điều chỉnh dịng đến 3A - Module nguồn không sử dụng cách ly - Nguồn đầu vào từ 4V - 35V - Nguồn đầu ra: 1V - 30V - Dịng Max: 3A - Kích thước mạch: 53 (mm) x26 (mm) 29 Hình 3.9: Mạch giảm áp LM2596 3.4.4 MẠCH CẦU H Mạch cầu H đóng vai trò trung gian giao tiếp vi điều khiển cấu chấp hành đề tài động Tín hiệu vi điều khiển cách ly với tín hiệu vào MOSFET cơng st opto quang nhằm tránh xung đột điện áp gây hư hỏng vi điều khiển Hình 3.10: Mạch vi điều khiển mạch cầu H 30 Khi động mắc vào OUT1 OUT2 đồng thời ngõ vào INT mức cao, đầu vào INT2 mức thấp động quay phía trước Ngược lại đầu INT1 mức thấp INT2 mức cao động quay phía sau Tương tự cho động lại Trong trường hợp hai mức cao mức thấp động dừng L298N 2.5A Mina: Mạch điều khiển động dòng 2.5A với dây silicon Điện áp nguồn từ 2V - 10V, điều khiển hai động DC động bước pha dây lúc Nó điều khiển tiến, lùi tốc độ với bảo vệ nhiệt phục hồi tự động: - Điều khiển động cầu H hai chiều, điều khiển hai động DC động bước hai pha dây lúc - Điện áp 2V - 10V - Dòng điện làm việc đơn 2.5A, Dòng điện chờ thấp (dưới 0,1ua) - Mạch bảo vệ nhiệt tích hợp (tSD) - Kích thước sản phẩm: 31x32x5mm (chiều dài, chiều rộng chiều cao), Kích thước nhỏ, thích hợp để lắp ráp xe - Trọng lượng: 11g Hình 3.11: Mạch cầu H L298N 3.4.5 CẢM BIẾN GĨC 31 Cảm biến góc GY-521 MPU6050: Là cảm biến sáu trục, có chứa gia tốc trục quay hồi chuyển trục Hoạt động với 3.3V giao tiếp I2C với tốc độ tối đa 400KHz Các cảm biến bên MPU-6050 sử dụng chuyển đổi tương tự – số (Anolog to Digital Converter – ADC) 16-bit cho kết chi tiết góc quay, tọa độ… Với 16-bit bạn có 2^16 = 65536 giá trị cho cảm biến Tùy thuộc vào yêu cầu bạn, cảm biến MPU-6050 hoạt động chế độ tốc độ xử lý cao chế độ đo góc quay xác (chậm hơn) MPU-6050 có khả đo phạm vi: Con quay hồi chuyển: ± 250 500 1000 2000 dps Gia tốc: ± ± ± ± 16g Hơn nữa, MPU6050 có sẵn đệm liệu 1024 byte cho phép vi điều khiển phát lệnh cho cảm biến, nhận liệu sau MPU-6050 tính tốn xong Hình 3.12: MPU6050 3.5 SƠ ĐỒ KẾT NỐI 32 Hình 3.13: Mạch điện kết nối cảm biến với vi điều khiển 33 3.6 MƠ HÌNH THỰC Hình 3.14: Mơ hình thực xe cân 34 CHƯƠNG KẾT QUẢ Kết thu sau nghiên cứu:: - Hoàn thành nhiệm vụ thiết kế thi cơng mơ hình robot hồn chỉnh - Sử dụng lọc Kalman cho MPU – 6050 nhằm tăng tính ổn định: + Tín hiệu đỏ tín hiệu có sử dụng lọc Kalman + Tín hiệu xanh tín hiệu khơng sử dụng lọc Kalman Hình 4.1: Lọc Kalman cho MPU – 6050 - Robot giữ thăng dùng giải thuật LQR chưa hồn thiện Hình 4.2: Đáp ứng hệ thống 35 Hướng phát triển đề tài: - Sử dụng giải thuật Fuzzy-PID thay cho giải thuật LQR - Mơ hình phát triển để điều khiển thơng qua máy tính nhờ kết nối bluetooth giao diện máy tính thân thiện trực quan - Tối ưu hóa khối lượng, thuật tốn để mơ hình hoạt động trơn tru, linh hoạt, dễ điều khiển, tiết kiệm lượng - Có thể gắn thêm camera gps để robot định vị, ghi hình, xử lý ảnh tự hoạt động không gian lớn - Mơ hình robot bánh tự cân tiền đề để phát triển thành xe bánh tự cân chở người di chuyển linh hoạt 36 TÀI LIỆU THAM KHẢO [1] KS Mai Tuấn Đạt, Xe Hai Bánh Tự Cân Bằng Di Chuyển Trên Địa Hình Phẳng, Đại học Bách Khoa thành phố Hồ Chí Minh, năm 2005 [2] ThS Nguyễn Đình Phú, Vi Xử Lý, Đại học Sư Phạm Kỹ Thuật thành phố Hồ Chí Minh, năm 2013 [3] ThS Huỳnh Thái Hoàng, Lý Thuyết Điều Khiển Tự Động, NXB Đại Học Quốc Gia, năm 2005 [4] Cộng đồng Arduino Việt Nam Tác giả chịu trách nhiệm viết: Họ tên: Nguyễn Hưng, Hồ Lê Long Thiên Đơn vị: Khoa Đào Tạo Chất Lượng Cao, trường Đại học Sư Phạm Kỹ Thuật Điện thoại: 0338 432 252 Email: 17151086@student.hcmute.edu.vn, 17151132@student.hcmute.edu.vn 37 PHỤ LỤC #include #include "Kalman.h" #define ToRad PI/180 #define ToDeg 180/PI Kalman kalman; //Kalman filter define: kalman.getAngle(pitch, gyrorate, dt); float loopStartTimer,loop_time; const double FIX_loop_time = 12500; uint32_t timer; //Timer for kalman filter psi angle; uint8_t i2cData[14]; volatile long leftencoder; //read left encoder value volatile long righencoder; //read right encoder value //LQR data// long PWML, PWMR; // PWM output for H-Brigde float k1, k2, k3, k4, k5, k6;// The factor of K maxtrix bool falldown;// Run = true; Stop = false; float theta, psi, phi; float thetadot, psidot, phidot; float thetaold, psiold, phiold; float leftvolt; //output volt left motor in LQR float righvolt; //output volt right motor in lQR uint32_t timerloop, timerold; //MPU6050 Data// float mpudata; //Save psi angle ( Y axis) float AcX, AcZ; float Gyro; void setup() { Serial.begin(9600); //Enable Serial Communications k1 = 35; k2 = 5; k3 = 400; k4 = 15; k5 = 27; k6 = 2.3; //SET PWM FREQUENCY 31 kHz TCCR2B = TCCR2B & B11111000 | B00000001;//Pin & Pin 10 38 //Motor control Pin// pinMode(8,OUTPUT); //PWM Pin ENA pinMode(11,OUTPUT); //PWM Pin ENB pinMode(28,OUTPUT); //Left Motor Pin INT1 pinMode(26,OUTPUT); //Left Motor Pin INT2 pinMode(24,OUTPUT); //Right Motor Pin INT3 pinMode(22,OUTPUT); //Right Motor Pin INT4 //Encoder Pin// pinMode(1,INPUT_PULLUP);// CHÂN NGẮT LEFT pinMode(4,INPUT_PULLUP);// CHÂN NGẮT RIGHT pinMode(2,INPUT_PULLUP);// CHÂN ĐỌC ENCODER LEFT A pinMode(18,INPUT_PULLUP);// CHÂN ĐỌC ENCODER RIGHT A pinMode(3,INPUT_PULLUP);// CHÂN ĐỌC ENCODER LEFT B pinMode(19,INPUT_PULLUP);// CHÂN ĐỌC ENCODER RIGH B attachInterrupt(1, left_isr, FALLING); attachInterrupt(4, righ_isr, FALLING); //Data MPU6050// Wire.begin(); Wire.setClock(400000UL); // Set I2C frequency to 400kHz i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz i2cData[1] = 0x00;// Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, KHz sampling i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode while (i2cRead(0x75, i2cData, 1)); if (i2cData[0] != 0x68) { Serial.print(F("Error reading sensor")); while (1); } delay(100); // Wait for sensor to stabilize while (i2cRead(0x3B, i2cData, 6)); AcX = (i2cData[0]