HD5 nguyễn văn trường nghiên cứu thiết kế mô hình xe hai bánh tự cân bằng

0 BỘ CÔNG THƯƠNG TRƯỜNG ĐẠI HỌC CÔNG NGHIỆP HÀ NỘI KHOA CƠ KHÍ - - ĐỒ ÁN TỐT NGHIỆP Đề tài: NGHIÊN CỨU, THIẾT KẾ MƠ HÌNH XE HAI BÁNH TỰ CÂN BẰNG Giáo viên hướng dẫn: TS Nguyễn Văn Trường Nhóm sinh viên: Nguyễn Lê Thành 2018604875 Nguyễn Đức Thụy 2018603596 Đỗ Xuân Viên 2018604412 Lớp: Cơ điện tử Khóa: 13 Hà Nội - 2022 I I I I LỜI CẢM ƠN Trong suốt trình thực đề tài, gặp phải nhiều khó khăn giúp đỡ, hỗ trợ kịp thời từ quý Thầy Cô bạn nên đồ án hoàn thành tiến độ Em xin chân thành cảm ơn thầy Nguyễn Văn Trường tận tình hướng dẫn, bảo kinh nghiệm quý báu hỗ trợ phương tiện thí nghiệm suốt q trình tìm hiểu, nghiên cứu đề tài Mặc dù nhóm thực đề tài cố gắng hoàn thiện đồ án, trình soạn thảo kiến thức cịn hạn chế nên cịn nhiều thiếu sót Nhóm thực đề tài mong nhận đóng góp ý kiến q thầy bạn sinh viên Sau nhóm thực xin chúc Thầy cô sức khoẻ, thành công tiếp tục đào tạo sinh viên giỏi đóng góp cho đất nước Chúc bạn sức khỏe, học tập thật tốt để không phụ cơng lao Thầy Cơ giảng dạy Nhóm thực xin chân thành cảm ơn Trân trọng Người thực Nguyễn Lê Thành Nguyễn Đức Thuỵ Đỗ Xuân Viên I NHẬN XÉT CỦA GIÁO VIÊN Hà Nội, ngày …tháng …năm 2022 Giáo viên phản biện (Ký ghi rõ họ tên) II MỤC LỤC DANH MỤC HÌNH ẢNH V DANH MỤC BẢNG BIỂU VII MỞ ĐẦU CHƯƠNG GIỚI THIỆU CHUNG VỀ XE HAI BÁNH TỰ CÂN BẰNG 1.1 Đặt vấn đề 1.1.1 Lịch sử phát triển hình thành 1.1.2 Thế xe hai bánh tự cân 1.1.3 Tại phải thiết kế xe hai bánh tự cân 1.1.4 Ưu điểm robot hai bánh tự cân 1.1.5 Khả ứng dụng hiên mơ hình xe hai bánh tự cần 1.1.6 Tổng quan xe hai bánh tự cân nước giới 1.2 Mục tiêu đề tài 12 1.3 Đối tượng nghiên cứu 13 1.4 Phương pháp nghiên cứu 13 CHƯƠNG CƠ SỞ LÝ THUYẾT 14 2.1 Nguyên lý hoạt động robot hai bánh tự cân 14 2.2 Bài toán động học robot 14 2.3 Đặc tính động lực học 15 2.4 Bộ điều khiển PID 21 2.5 Bộ điều khiển tối ưu LQR 25 2.5.1 Bộ định thời liên tục LQR 26 2.5.2 Bộ định thời rời rạc LQR 26 2.6 Sơ đồ nguyên lý 27 2.7 Thơng số tính chọn linh kiện cho khối 28 2.7.1 Khối xử lý trung tâm 28 2.7.2 Khối cảm biến 30 2.7.3 Khối ngõ (Động cơ) 31 2.7.4 Khối điều khiển động 32 2.7.5 Khối nguồn 34 CHƯƠNG THIẾT KẾ, XÂY DỰNG MÔ HÌNH XE HAI BÁNH TỰ CÂN BẰNG 36 III 3.1 Tính tốn thiết kế hệ thống khí 36 3.1.1 Thiết kế xây dựng khung hệ thống 36 3.1.2 Tính chọn động 36 3.2 Tính toán thiết kế hệ thống điều khiển 38 3.2.1 Sơ đồ khối hệ thống điều khiển 38 3.2.2 Sơ đồ dây hệ thống 39 3.2.3 Kết nối hệ thống 39 3.2.4 Lưu đồ thuật toán điều khiển 40 3.3 Tính tốn thiết kế điều khiển 41 3.3.1 Thiết kế điều khiển PID 41 3.3.2 Thiết kế điều khiển LQR 45 3.4 Chế tạo thử nghiệm hệ thống 56 3.4.1 Chuẩn bị vật liệu 56 3.4.2 Các bước thiết kế chi tiết 56 3.5 Thử nghiệm hệ thống 57 3.5.1 Môi trường thử nghiệm 57 CHƯƠNG KẾT LUẬN VÀ ĐỊNH HƯỚNG PHÁT TRIỂN 58 4.1 Kết đạt Kết thiết kế thực tế 58 4.2 Hạn chế đề tài 60 4.3 Định hướng phát triển 60 IV DANH MỤC HÌNH ẢNH Hình 1-1 :Mơ tả ngun lý giữ thăng Hình 1-2: Mơ tả cách bắt đầu di chuyển Hình 1-3: Robot dạng bánh xe di chuyển địa hình phẳng Hình 1-4: Robot dạng bánh xe xuống dốc Hình 1-5: Robot dạng bánh xe lên dốc Hình 1-6: Robot bánh di chuyển địa hình khác bảo tồn thăng Hình 1-7: Robot hai bánh cân nBot Hình 1-8: Xe hai bánh cân Balance bot Hình 1-9: Balancing robot (Bbot ) 10 Hình 1-10: JOE robot 11 Hình 1-11: Balibot robot tầng mạch 12 Hình 2-1: Nguyên lý hoạt động xe bánh tự cân 14 Hình 2-2: Mơ hình robot bánh tự cân mặt phẳng 16 Hình 2-3: Bộ điều khiển PID 21 Hình 2-4: Sơ đồ hệ thống điều khiển LQR 25 Hình 2-5: Sơ đồ điều khiển 26 Hình 2-6: Sơ đồ nguyên lý hệ thống 27 Hình 2-7: Vi điều khiển Arduino Nano 28 Hình 2-8: MPU6050-GY521 30 Hình 2-9: Động DC 25GA370 31 Hình 2-10: Module điều khiển động L298N 32 Hình 2-11: Mạch cầu H IC L298N 32 Hình 2-12: Mạch điều khiển dòng điện qua động IC L298N 33 Hình 2-13: Sơ đồ tổng quát mạch cầu H sử dụng transistor BJT 33 Hình 2-14: Pin Lithium 18650 34 Hình 3-1: Mơ hình khung xe hai bánh tự cân 36 Hình 3-2: Động DC J25GA370 37 Hình 3-3: Sơ đồ khối tổng quát hệ thống 38 Hình 3-4: Sơ đồ dây cử hệ thông 39 Hình 3-5: Lưu đồ điều khiển hệ thống 40 Hình 3-6:Khối mơ bánh xe robot 42 Hình 3-7: Khối mơ khung robot 42 Hình 3-8: Kết nối bánh xe khung robot với khối tạo chuyển động 43 Hình 3-9: Kết nối với điều khiển PID 43 Hình 3-10: Kết mô hệ thống 43 Hình 3-11: Giá trị thông số PID 44 Hình 3-12:Tín hiệu vào điều khiển 45 Hình 3-13:Dây dựng điều khiển LQR 45 Hình 3-14: Khai báo thơng số 51 Hình 3-15: Nhập ma trận A,B 51 Hình 3-16: Nhập ma trận Q,R 52 V Hình 3-17: Kết ma trận A,B 52 Hình 3-18: Kết ma trận K 52 Hình 3-19: Mơ hình hóa hệ xe bánh tự cân 53 Hình 3-20: Mơ hình hóa hệ xe bánh tự cân 53 Hình 3-21: Nhập hàm h1 54 Hình 3-22: Nhập hàm 54 Hình 3-23: Nhập hàm h3 55 Hình 3-24: Đồ thị tín hiệu mơ Simulink hệ thống 55 Hình 4-1: Kết thiết kế thực tế 58 Hình 4-2: Kết thiết kế thực tế 58 Hình 4-3: Mơ hình chạy thực tế 59 VI pinMode(rightMotor1Pin, OUTPUT); pinMode(rightMotor2Pin, OUTPUT); pinMode(enaPin, OUTPUT); pinMode(enbPin, OUTPUT); setMotors(0,0); init_PID(); } void loop() { //đọc bluetooth //thiết lập góc mục tiêu,góc lệch trái,góc lệch phải if (bluetooth.available()) { char ch = bluetooth.read(); Serial.println(ch); if(ch == 'A') //tiến { Serial.println("UP"); targetAngle = targetAngleUp; offsetLeft = 0; offsetRight = 0; } else if(ch == 'B') //lùi { Serial.println("Down"); targetAngle = targetAngleDown; offsetLeft = 0; offsetRight = 0; } else if(ch == 'D') //trái { Serial.println("Left"); 67 targetAngle = targetAngleBalance; offsetLeft = 40; offsetRight = -40; } else if(ch == 'C') //phải { Serial.println("Right"); targetAngle = targetAngleBalance; offsetLeft = -40; offsetRight = 40; } else //stop { Serial.println("Stop"); targetAngle = targetAngleBalance; offsetLeft = 0; offsetRight = 0; } } if (!dmpReady) return; // chờ ngắt MPU (các) gói bổ sung khả dụng while (!mpuInterrupt && fifoCount < packetSize) { if (mpuInterrupt && fifoCount < packetSize) { // try to get out of the infinite loop fifoCount = mpu.getFIFOCount(); } } // đặt lại cờ ngắt nhận INT_STATUS byte mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); 68 // nhận số FIFO fifoCount = mpu.getFIFOCount(); // kiểm tra overflow if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) { mpu.resetFIFO(); fifoCount = mpu.getFIFOCount(); Serial.println(F("FIFO overflow!")); // kiểm tra ngắt liệu DMP sẵn sàng (điều xảy thường xuyên) } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) { while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); //đợi độ dài liệu có sẵn xác, thời gian chờ RẤT ngắn mpu.getFIFOBytes(fifoBuffer, packetSize); // đọc gói từ FIFO // theo dõi số lượng FIFO trường hợp có> gói có sẵn fifoCount -= packetSize; #ifdef OUTPUT_READABLE_YAWPITCHROLL // hiển thị góc Euler theo độ mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); currentAngle = ypr[2] * 180/M_PI + angle_offset; //Serial.println(currentAngle); if(count_imu_ready > 500)//5s { digitalWrite(LED_PIN, HIGH); imu_ready = true; } count_imu_ready++; #endif } 69 } // ISR gọi sau mili giây ISR(TIMER1_COMPA_vect) { if(imu_ready && currentAngle > -30 && currentAngle < 30){ // Đọc góc nghiêng error = currentAngle - targetAngle; // iSum += Ki*(error)*sampleTime; iSum = constrain(iSum, -255, 255); //tính tốn đầu từ giá trị P, I D motorPower = Kp*(error) + iSum + Kd*(error-prevError)/sampleTime; motorPower = constrain(motorPower, -255, 255); targetAngle = (motorPower > Limit_Speed) ? (targetAngle - 0.1) : targetAngle; targetAngle = (motorPower < -Limit_Speed) ? (targetAngle + 0.1) : targetAngle; setMotors(motorPower + offsetLeft, motorPower + offsetRight); prevError = error; } else { setMotors(0, 0); } } void init_PID() { // khởi tạo timer cli(); TCCR1A = 0; // đặt toàn ghi TCCR1A thành TCCR1B = 0; // tương tự cho TCCR1B // cài đặt ghi so sánh để đặt thời gian lấy mẫu 5ms OCR1A = 19999; // bật chế độ CTC TCCR1B |= (1